#ifndef QUATERNIONTEM_H
#define QUATERNIONTEM_H

#include "imMaths.h"
#include "Matrix4.h"
#include "Vector3.h"
#include "Angle.h"

template <typename Real>
struct QuaternionTem {
public:
	union {
		struct {
			Real r; // Real
			Real i, j, k; // Imaginary
		};
		Real Data[4];
	};

	QuaternionTem<Real>(void){ i=0.0, j=0.0, k=0.0, r=1.0; };
	QuaternionTem<Real>(Real _r, Real _i, Real _j, Real _k) {
		r = _r;
		i = _i;
		j = _j;
		k = _k;
	}
	QuaternionTem<Real>(const AngleTem<Real> &a) {
		Real cX = cos(0.5*a.p);
		Real cY = cos(0.5*a.y);
		Real cZ = cos(0.5*a.r);

		Real sX = sin(0.5*a.p);
		Real sY = sin(0.5*a.y);
		Real sZ = sin(0.5*a.r);

		Real cYcZ, sYsZ, cYsZ, sYcZ;

		cYcZ = cY * cZ;
		sYsZ = sY * sZ;
		cYsZ = cY * sZ;
		sYcZ = sY * cZ;

		// and now compute quaternion
		r = cX * cYcZ + sX * sYsZ;
		i = sX * cYcZ - cX * sYsZ;
		j = cX * sYcZ + sX * cYsZ;
		k = cX * cYsZ - sX * sYcZ;
	}

	void Normalize() {
		Real d = r*r+i*i+j*j+k*k;

		if(d == 0) {
			r = 1.0f;
			return;
		};

		d = 1.0f / sqrt(d);
		r *= d;
		i *= d;
		j *= d;
		k *= d;
	}

	void GetMatrix(Matrix4Tem<Real> *m) {
		Real ri, rj, rk, ii, jj, jk, ij, ik, kk, i2, j2, k2;

		m->Identity();

		i2 = i + i;
		j2 = j + j;
		k2 = k + k;

		ii = i * i2;
		ij = i * j2;
		ik = i * k2;

		jj = j * j2;
		jk = j * k2;
		kk = k * k2;

		ri = r * i2;
		rj = r * j2;
		rk = r * k2;

		m->_11 = 1.0 - (jj + kk);
		m->_12 = ij - rk;
		m->_13 = ik + rj;

		m->_21 = ij + rk;
		m->_22 = 1.0 - (ii + kk);
		m->_23 = jk - ri;
		
		m->_31 = ik - rj;
		m->_32 = jk + ri;
		m->_33 = 1.0 - (ii + jj);
	}

	void AddScaledVector(const Vector3Tem<Real> &v, Real Scale) {
		Quaternion<Real> q(0,
		v.x * Scale,
		v.y * Scale,
		v.z * Scale);
		q *= *this;
		r += q.r * 0.5;
		i += q.i * 0.5;
		j += q.j * 0.5;
		k += q.k * 0.5;
	}

	Vector3 Forward(void) {
		Matrix4Tem<Real> m;
		GetMatrix(&m);

		return Vector3Tem<Real>(m._13, m._23, m._33);
	}
	Vector3 Right(void) {
		Matrix4Tem<Real> m;
		GetMatrix(&m);

		return Vector3Tem<Real>(m._11, m._21, m._31);
	}
	Vector3 Up(void) {
		Matrix4Tem<Real> m;
		GetMatrix(&m);

		return Vector3Tem<Real>(m._12, m._22, m._32);
	}

	QuaternionTem<Real> operator * (const QuaternionTem<Real> &b) {
		QuaternionTem<Real> qResult = QuaternionTem<Real>();

		qResult.r = r*b.r - i*b.i - j*b.j - k*b.k;
		qResult.i = r*b.i + i*b.r - j*b.k - k*b.j;
		qResult.j = r*b.j - i*b.k + j*b.r - k*b.k;
		qResult.k = r*b.k + i*b.j - j*b.i + k*b.k;

		return qResult;
	}
	void operator *= (const QuaternionTem<Real> &b) {
		Real _r = r*b.r - i*b.i - j*b.j - k*b.k;
		Real _i = r*b.i + i*b.r - j*b.k - k*b.j;
		Real _j = r*b.j - i*b.k + j*b.r - k*b.k;
		Real _k = r*b.k + i*b.j - j*b.i + k*b.k;

		r = _r;
		i = _i;
		j = _j;
		k = _k;
	}
};

typedef QuaternionTem<float> Quaternion;
typedef QuaternionTem<float> Quaternionf;
typedef QuaternionTem<double> Quaterniond;

#endif